/*
1
recv msg from serial(aiui) and content 
send content to another app to decompress 
2
recv tts string and format the string to json,at last send the json to serial(aiui)
*/

#include <ros/ros.h> 
#include <serial/serial.h>  //ROS已经内置了的串口包 
#include <std_msgs/String.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Empty.h>
#include <time.h>


#include "../include/packet.hpp"

serial::Serial ser; //声明串口对象
uint16_t g_msgid = 0;
uint16_t g_pre_send_msgid = 0;
bool connected = false;
time_t send_time;
time_t recv_time;
//回调函数 
void write_callback(const std_msgs::String::ConstPtr& msg) 
{ 
    std::string tts_str = "{\"type\": \"tts\",\"content\": {\"action\": \"start\",\"text\":\""+ msg->data +"\"}}'";

    if (g_msgid == g_pre_send_msgid)//we cannot send same msgid, see:AIUI集成文档/c04/uartkit/04_uart_QA.html
        g_msgid ++;

    std::string send_str;
    send_str.push_back(0xa5);
    send_str.push_back(0x01);
    send_str.push_back(0x05);//type
    send_str.push_back(tts_str.length() & 0xff);//length
    send_str.push_back((tts_str.length() >> 8) & 0xff);//length
    send_str.push_back(g_msgid & 0xff);
    send_str.push_back((g_msgid >> 8) & 0xff);
    send_str += tts_str;
	char check_code = 0;
	for(int index = 0; index < send_str.length(); index++){
        // printf("%02x ", send_str[index] & 0xff);
		check_code += (send_str[index] & 0xff);
	}
	check_code = ~check_code + 1;
	send_str.push_back(check_code & 0xff);
    try {
        ser.write(send_str);   //发送串口数据 
    } catch (serial::IOException& e) {
        ROS_ERROR_STREAM(e.what());
        return;
    } catch (serial::SerialException& e) {
        ROS_ERROR_STREAM(e.what());
        return;
    }
    g_pre_send_msgid = g_msgid;
}

void qrcode_callback(const std_msgs::String::ConstPtr& msg) 
{ 
//     {
//         "type": "aiui_msg",
//         "content": {
//                 "msg_type": 8, //CMD_RESET_WAKEUP  重置AIUI唤醒状态
//                 "arg1": 0,
//                 "arg2": 0,
//                 "params": ""
//         }
// }
    std::string tts_str = "{\"type\": \"aiui_msg\",\"content\": {\"msg_type\":7,\"arg1\":0,\"arg2\":0,\"params\":\"qrcode\"}}'";

    if (g_msgid == g_pre_send_msgid)//we cannot send same msgid, see:AIUI集成文档/c04/uartkit/04_uart_QA.html
        g_msgid ++;

    std::string send_str;
    send_str.push_back(0xa5);
    send_str.push_back(0x01);
    send_str.push_back(0x05);//type
    send_str.push_back(tts_str.length() & 0xff);//length
    send_str.push_back((tts_str.length() >> 8) & 0xff);//length
    send_str.push_back(g_msgid & 0xff);
    send_str.push_back((g_msgid >> 8) & 0xff);
    send_str += tts_str;
	char check_code = 0;
	for(int index = 0; index < send_str.length(); index++){
        // printf("%02x ", send_str[index] & 0xff);
		check_code += (send_str[index] & 0xff);
	}
	check_code = ~check_code + 1;
	send_str.push_back(check_code & 0xff);
    try {
        ser.write(send_str);   //发送串口数据 
    } catch (serial::IOException& e) {
        ROS_ERROR_STREAM(e.what());
        return;
    } catch (serial::SerialException& e) {
        ROS_ERROR_STREAM(e.what());
        return;
    }
    g_pre_send_msgid = g_msgid;
}

void echo(unsigned char type, unsigned int msgid)
{
    std::string send_str;
    send_str.push_back(0xa5);
    send_str.push_back(0x01);
    send_str.push_back(0xff);
    send_str.push_back(0x04);
    send_str.push_back(0x00);
    send_str.push_back(msgid & 0xff);
    send_str.push_back((msgid >> 8) & 0xff);
    send_str.push_back(0xa5);
    send_str.push_back(0x00);
    send_str.push_back(0x00);
    send_str.push_back(0x00);

	char check_code = 0;
	for(int index = 0; index < send_str.length(); index++){
        // printf("%02x ", send_str[index] & 0xff);
		check_code += (send_str[index] & 0xff);
	}
	check_code = ~check_code + 1;
	send_str.push_back(check_code & 0xff);
    // printf("\ncheck_code = %02x\n", check_code & 0xff);

    try {
        ser.write(send_str);   //发送串口数据 
    } catch (serial::IOException& e) {
        ROS_ERROR_STREAM(e.what());
        return;
    } catch (serial::SerialException& e) {
        ROS_ERROR_STREAM(e.what());
        return;
    }
    g_msgid = msgid;
}

void heartbeat()
{
    std::string send_str;
    send_str.push_back(0xa5);
    send_str.push_back(0x01);
    send_str.push_back(0x01);
    send_str.push_back(0x04);
    send_str.push_back(0x00);
    send_str.push_back(g_msgid & 0xff);
    send_str.push_back((g_msgid >> 8) & 0xff);
    send_str.push_back(0xa5);
    send_str.push_back(0x00);
    send_str.push_back(0x00);
    send_str.push_back(0x00);

	char check_code = 0;
	for(int index = 0; index < send_str.length(); index++){        
		check_code += (send_str[index] & 0xff);
	}
	check_code = ~check_code + 1;
	send_str.push_back(check_code & 0xff);    

    try {
        ser.write(send_str);   //发送串口数据 
    } catch (serial::IOException& e) {
        ROS_ERROR_STREAM(e.what());
        return;
    } catch (serial::SerialException& e) {
        ROS_ERROR_STREAM(e.what());
        return;
    }
}

// static bool connected

int main (int argc, char** argv) 
{ 
    //初始化节点 
    ros::init(argc, argv, "serial_example_node"); 
    //声明节点句柄 
    ros::NodeHandle nh; 
  
    //订阅主题，并配置回调函数 
    ros::Subscriber write_sub = nh.subscribe("tts", 1000, write_callback);
    ros::Subscriber qrcode_sub = nh.subscribe("qrcode_wakeup", 1000, qrcode_callback); 
    //发布主题 
    ros::Publisher read_pub = nh.advertise<std_msgs::String>("aiuimsg", 1000);
    ros::Publisher tts_result_pub = nh.advertise<std_msgs::String>("tts_result", 10);

    PacketFinderBase packet_finder;
    ecl::PushAndPop<unsigned char> stx(2, 0);
    ecl::PushAndPop<unsigned char> etx(1);
    stx.push_back(0xa5);
    stx.push_back(0x01);
    packet_finder.configure(stx, etx, 1, 2, 2, 2048, 1, true);    
    
    send_time = recv_time = time(NULL);

    //指定循环的频率 
    ros::Rate loop_rate(60); 
    while(ros::ok())
    {
        if (time(NULL) - recv_time > 10) {
            ROS_ERROR_STREAM("AIUI disconnect!!");
            connected = false;
            ser.close();
        }
        if (!connected) {
            try {
                //设置串口属性，并打开串口 
                ser.setPort("/dev/arduino"); 
                ser.setBaudrate(115200); 
                serial::Timeout to = serial::Timeout::simpleTimeout(1000); 
                ser.setTimeout(to);
                ser.open(); 
            } catch (serial::IOException& e) {
                ROS_ERROR_STREAM("Unable to open port "); 
                connected = false;
            }
        
            //检测串口是否已经打开，并给出提示信息 
            if(ser.isOpen()) { 
                ROS_INFO_STREAM("Serial Port initialized");
                connected = true;
                recv_time = time(NULL);
            } else { 
                connected = false;
            }
        }
        else {
            try {
                if (time(NULL) - send_time > 5) {
                    heartbeat();
                    send_time = time(NULL);
                }
                if (ser.available()) {
                    std_msgs::String result;            
                    unsigned int numOfData2Read = packet_finder.numberOfDataToRead();            
                    std::string recv_str = ser.read(numOfData2Read);    

                    if (packet_finder.update((unsigned char *)recv_str.c_str(), recv_str.length())) {
                        unsigned char type = packet_finder.getMsgType();
                        unsigned int msgid = packet_finder.getMsgId();
                        
                        if (type == 0x01) {
                            
                        } else if (type == 0x04) {
                            std::string msg;
                            packet_finder.getPayload(msg);

                            result.data = msg;
                            read_pub.publish(result);                                        
                        } else if(type == 0x05) {
                            std::string msg;
                            packet_finder.getPayload(msg);
                            // printf("msg = %s\n", msg.c_str());
                            // ROS_INFO_STREAM("to do type");
                        } else if(type == 0x2a) {
                            std::string msg;
                            packet_finder.getPayloadAscii(msg);
                            ROS_INFO_STREAM(msg);
                            result.data = msg;
                            // char state = msg[0];
                            // if (state == 0)
                            //     result.data = "start";                        
                            // else
                            //     result.data = "stop";
                            tts_result_pub.publish(result);
                        } else if (type == 0xff) {

                        }
                        if (type != 0xff)//if type is not 0xff(确认消息) 
                            echo(type, msgid);
                        
                        recv_time = time(NULL);
                    }
                }
            } catch (serial::IOException& e) {
                ROS_ERROR_STREAM(e.what());
                connected = false;
                // return -1;
            } catch (serial::SerialException& e) {
                ROS_ERROR_STREAM(e.what());
                // return;
                connected = false;
            }
        }
        ros::spinOnce(); 
        loop_rate.sleep();  
    } 
} 